#ifndef FINDARMORBOXES_H
#define FINDARMORBOXES_H

#include "opencv2/opencv.hpp"
#include <iostream>
#include <vector>
#include "Classifier/classifier.h"
#include "constValues.h"
#include "Camera.h"

#define TEST 1

using namespace cv;
using namespace std;
using namespace robowalker;

enum ArmorBoxType
{
    ARMORBOX_SMALL = 0,
    ARMORBOX_BIG = 1,
};

struct ArmorBox
{
public:
	int color;
	int number;
	Point2f vertices[4];
	RotatedRect light_blob_left;
	RotatedRect light_blob_right;
public:
    ArmorBox();
	ArmorBox(int color, int number, const RotatedRect& lb1, const RotatedRect& lb2);
    void setNull();
    bool isNull() const;
	double getHeight() const;
	double getWidth() const; 
	Point2f getCenter() const;
	ArmorBoxType getArmorBoxType() const;
};

class ArmorBoxesDetector
{
public:
	enum class Enemy
	{
		Blue = 0,
		Red
	};

private:
	Classifier classifier;
	int height;
	int width;
	
	bool is_show = false;

	// parameters
	Enemy enemy;
	double downsampled_k;
	double enlarge_k_findBlobs;
	int number_window_w;
	int number_window_h;
	double clipping_kw;
	double clipping_kh;

	double max_lwrate;
	double max_angle;
	double min_angle;
	double min_area;
	double max_area;

	double max_length_radio;
	double max_length_diff;
	double max_angle_diff;
	double max_box_lwrate;
	double min_box_angle;

	uchar min_blue_h;
	uchar max_blue_h;
	uchar min_red_h;
	uchar max_red_h;
	uchar min_s;
	uchar max_s;
	uchar min_v;
	
	uchar max_s2;
	uchar min_v2;

	inline bool isBlue(const uchar hsv[3]);
	inline bool isRed(const uchar hsv[3]);
	bool (ArmorBoxesDetector::*isEnemyColor)(const uchar hsv[3]);

	inline bool isBlueBlob(const cv::Mat& src);
	inline bool isRedBlob(const cv::Mat& src);
	
	bool (ArmorBoxesDetector::*isEnemyBlob)(const cv::Mat& src);

	bool isValidLightBlob(const cv::RotatedRect& rect);
	bool isCoupleLightBlobs(const cv::RotatedRect& rect_i, const cv::RotatedRect& rect_j);

	//void findBlobsPreproc(const Mat& src, Mat& dst);
	void findBlobs(const cv::Mat& src, vector<cv::RotatedRect>& rst);
	
	bool matchArmorBoxes(const cv::Mat& src, const vector<cv::RotatedRect>& light_blobs, vector<ArmorBox>& armor_boxes);
public:

	void setEnemy(const string& str);

	ArmorBoxesDetector(int width, int height, const string& modelfile, int maxThreadNum, const string& paramfile);
	
	void operator()(const cv::Mat& src, vector<ArmorBox> &rst, vector<cv::RotatedRect> &blobs, vector<ArmorBox> &suspects);
	
	void draw(const cv::Mat& src, cv::Mat &dst, const vector<ArmorBox> &rst, const vector<cv::RotatedRect> &blobs, const vector<ArmorBox> &suspects);
	ArmorBoxesDetector(const ArmorBoxesDetector&) = delete;

	ArmorBox SelectArmorBoxes(vector<ArmorBox>& rst, ArmorBox& lastArmorBox, Camera& cam);

	void SelectArmorBoxesDepthFilter(vector<ArmorBox>& rst, Camera& cam); 

	ArmorBox SelectArmorBoxesWithMaxHeight(const vector<ArmorBox>& rst);

    pair<ArmorBox, double> SelectArmorBoxesWithMaxCorrelation(const vector<ArmorBox>& rst, const Point2f& targetCenter );

	int detectState;  

	int getDetectState() const ; 

	void set_show(bool flag);
};
#endif